#include <fstream>
#include <assert.h>
#include <map>
#include <set>
#include "astar.h"
#include "node.h"
#include "map.h"
#include "graph.h"

typedef std::set<NodeIndex> ClosedSet;

void getCorners(const Map& map, Graph& graph, int start/* = 0*/, int end/* = -1*/)
{
  if (end == -1 || end > map.size())
    end = map.size();

  for (int i = start; i < end; ++i)
  {
    int row, col;
    map.getRowCol(i, row, col);

    // Skip cells that aren't open
    if (map.isBarrier(row, col)) continue;

    // Build the mask for what the area around this cell looks like
    //
    //   1   2   4
    //   8   X   16
    //   32  64 128
    //
    short mask = 0;
    if (map.isBarrier(row-1, col-1))  mask |= 1;
    if (map.isBarrier(row-1, col))    mask |= 2;
    if (map.isBarrier(row-1, col+1))  mask |= 4;
    if (map.isBarrier(row,   col-1))  mask |= 8;
    if (map.isBarrier(row,   col+1))  mask |= 16;
    if (map.isBarrier(row+1, col-1))  mask |= 32;
    if (map.isBarrier(row+1, col))    mask |= 64;
    if (map.isBarrier(row+1, col+1))  mask |= 128;

    // Look for open corners around this cell.
    static const short mask_11 = 11;
    static const short mask_22 = 22;
    static const short mask_104 = 104;
    static const short mask_208 = 208;

    if ((mask & mask_11) == 1   || (mask & mask_22) == 4 ||
        (mask & mask_104) == 32 || (mask & mask_208) == 128)
    {
      // This is a corner, so add it to the graph
      graph.addNode(NodeIndex(col, row));
    }
  }
}

bool isInSet(Node* node, const ClosedSet& set)
{
  return (set.find(node->getIndex()) != set.end());
}

bool isInSet(Node* node, const NodeMap& set)
{
  return (set.find(node->getIndex()) != set.end());
}

Node* getNodeWithLowestF(NodeMap& set)
{
  NodeMap::iterator lowest = set.begin();
  if (set.size() != 1)
  {
    // Find the Node with the lowest f()
    NodeMap::iterator itr = lowest;
    for (; itr != set.end(); ++itr)
      if (itr->second->f < lowest->second->f) lowest = itr;
  }

  // Remove the node from the set
  Node* node = lowest->second;
  set.erase(lowest);

  return node;
}

void astar(Graph& graph, const NodeIndex& startIndex, const NodeIndex& goalIndex, Path& path)
{
	typedef std::map<NodeIndex, NodeIndex> RevPath;

	NodeMap open_set;
	ClosedSet closed_set;
	RevPath came_from;

	Node* startNode = graph[startIndex];
  Node* goalNode = graph[goalIndex];

  // Make sure both of the are valid Nodes
  assert(startNode && goalNode);

  startNode->g = 0;
  startNode->computeH(goalNode);
  startNode->updateF();
  open_set[startNode->getIndex()] = startNode;;

  while (!open_set.empty())
  {
    Node* node = getNodeWithLowestF(open_set);

    // See if we're at the goal
    if (node == goalNode)
    {
      path.push_back(goalNode->getIndex());
      break;
    }

    // Add this node to the closed set
    closed_set.insert(node->getIndex());

    // Visit all children of the current node
    NodeMap& neighbors = node->getNeighbors();
    NodeMap::iterator itr = neighbors.begin();
    while (itr != neighbors.end())
    {
      Node* neighbor = (itr++)->second;

      // See if this child is in the closed list
      if (isInSet(neighbor, closed_set)) continue;

      float tentative_g = node->g + neighbor->computeG(node);
      bool tentative_is_better = false;

      // See if child is in open set
      if (isInSet(neighbor, open_set) == false)
      {
        open_set[neighbor->getIndex()] = neighbor;
        tentative_is_better = true;
      }
      else if (tentative_g < neighbor->g)
      {
        tentative_is_better = true;
      }

      if (tentative_is_better)
      {
        came_from[neighbor->getIndex()] = node->getIndex();
        neighbor->g = tentative_g;
        neighbor->h = neighbor->computeH(goalNode);
        neighbor->updateF();
      }
    } // while
  } // while

  // Travel back through the nodes to get the path
  RevPath::iterator itr = came_from.find(goalNode->getIndex());
  while (itr != came_from.end())
  {
    path.push_back(graph[itr->second]->getIndex());
    itr = came_from.find(itr->second);
  }
}
